#include <stdio.h>
#include <stdlib.h>
#include "imm.h"

int main()
{
    // 初始化IMM滤波器
    Imm *imm = imm_cvt();
    if (!imm)
    {
        printf("创建IMM滤波器失败\n");
        return 1;
    }

    // 创建示例观测数据（6x1向量）
    Matrix *Z = create_matrix(6, 1);
    Z->data[0][0] = 10.0; // x位置
    Z->data[1][0] = 2.0;  // x速度
    Z->data[2][0] = 5.0;  // y位置
    Z->data[3][0] = 1.0;  // y速度
    Z->data[4][0] = 3.0;  // z位置
    Z->data[5][0] = 0.5;  // z速度

    // 执行10步滤波示例
    for (int i = 0; i < 10; i++)
    {
        printf("===  %d step filter ===\n", i + 1);

        // 更新观测值（简单模拟移动）
        Z->data[0][0] += Z->data[1][0] * DT;
        Z->data[2][0] += Z->data[3][0] * DT;
        Z->data[4][0] += Z->data[5][0] * DT;
        printf("===  %d step filter center===\n", i + 1);
        // 执行IMM滤波
        Matrix *probabilities = imm_filt(imm, Z);

        // 打印各模型概率
        printf("模型概率:\n");
        for (int j = 0; j < imm->mode_cnt; j++)
        {
            printf("模型 %d: %.6f\n", j, probabilities->data[j][0]);
        }
        printf("\n");

        free_matrix(probabilities);
    }

    // 释放资源
    free_matrix(Z);
    free_imm(imm);

    return 0;
}